import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
import csv
from collections import deque

class PoseComparator(Node):
    def __init__(self):
        super().__init__('pose_comparator')
        self.gnss_pose_buffer = deque()
        self.ndt_pose_buffer = deque()
        self.pose_data = deque()
        self.gnss_msg = None

        self.gnss_subscription = self.create_subscription(
            PoseWithCovarianceStamped,
            '/sensing/gnss/pose_with_covariance',
            self.gnss_pose_callback,
            10)
        self.ndt_subscription = self.create_subscription(
            PoseWithCovarianceStamped,
            '/localization/pose_with_covariance',
            self.ndt_pose_callback,
            10)

    def gnss_pose_callback(self, msg):
        # if len(self.gnss_pose_buffer) > 10:
        #     self.gnss_pose_buffer.popleft()

        # self.gnss_pose_buffer.append(msg)
        self.gnss_msg = msg
        self.compare_poses()

    def ndt_pose_callback(self, msg):
        if len(self.ndt_pose_buffer) > 50:
            self.ndt_pose_buffer.popleft()
        self.ndt_pose_buffer.append(msg)
        # self.compare_poses()

    def compare_poses(self):
        # if not self.gnss_pose_buffer:
        #     print("GNSS pose buffer is empty")
        #     return
        if len(self.ndt_pose_buffer) == 0:
            print("NDT pose buffer is empty")
            return

        gnss_pose = self.gnss_msg
        # gnss_pose = self.gnss_pose_buffer[0]

        gnss_stamp = gnss_pose.header.stamp.sec * 1e9 + gnss_pose.header.stamp.nanosec
        
        while True:
            ndt_pose = self.ndt_pose_buffer[0]
            ndt_stamp = ndt_pose.header.stamp.sec * 1e9 + ndt_pose.header.stamp.nanosec
            
            gnss_x = gnss_pose.pose.pose.position.x
            gnss_y = gnss_pose.pose.pose.position.y
            ndt_x = ndt_pose.pose.pose.position.x
            ndt_y = ndt_pose.pose.pose.position.y

            x_diff = abs(gnss_x - ndt_x)
            y_diff = abs(gnss_y - ndt_y)

            # self.get_logger().info(f'gnss_stamp:{gnss_pose.header.stamp.sec}.{gnss_pose.header.stamp.nanosec}  ndt_stamp:{ndt_pose.header.stamp.sec}.{ndt_pose.header.stamp.nanosec}')

            if abs(gnss_stamp - ndt_stamp) < 1e6:
                self.pose_data.append((gnss_pose.header.stamp.sec, gnss_pose.header.stamp.nanosec, x_diff, y_diff, gnss_x, gnss_y, ndt_x, ndt_y))
                self.get_logger().info(f'Timestamp: {gnss_pose.header.stamp.sec}.{gnss_pose.header.stamp.nanosec}, X Diff: {x_diff}, Y Diff: {y_diff}, GNSS X: {gnss_x}, GNSS Y: {gnss_y}, NDT X: {ndt_x}, NDT Y: {ndt_y}')
                self.ndt_pose_buffer.popleft()
                break
            if len(self.ndt_pose_buffer) > 1:
                self.ndt_pose_buffer.popleft()
            else:
                self.get_logger().info(f'im break while')
                break

        # if abs(gnss_stamp - ndt_stamp) < 1e6:  # 1ms tolerance
        #     gnss_x = gnss_pose.pose.pose.position.x
        #     gnss_y = gnss_pose.pose.pose.position.y
        #     ndt_x = ndt_pose.pose.pose.position.x
        #     ndt_y = ndt_pose.pose.pose.position.y

        #     x_diff = abs(gnss_x - ndt_x)
        #     y_diff = abs(gnss_y - ndt_y)

        #     self.pose_data.append((gnss_pose.header.stamp.sec, gnss_pose.header.stamp.nanosec, x_diff, y_diff, gnss_x, gnss_y, ndt_x, ndt_y))
        #     self.get_logger().info(f'Timestamp: {gnss_pose.header.stamp.sec}.{gnss_pose.header.stamp.nanosec}, X Diff: {x_diff}, Y Diff: {y_diff}, GNSS X: {gnss_x}, GNSS Y: {gnss_y}, NDT X: {ndt_x}, NDT Y: {ndt_y}')
        #     self.gnss_pose_buffer.popleft()
        #     self.ndt_pose_buffer.popleft()
        # elif gnss_stamp < ndt_stamp:
        #     self.gnss_pose_buffer.popleft()
        #     self.get_logger().info(f'gnss_stamp < ndt_stamp: {gnss_stamp}   {ndt_stamp}')
        # elif gnss_stamp > ndt_stamp:
        #     self.ndt_pose_buffer.popleft()
        #     self.get_logger().info(f'gnss_stamp > ndt_stamp: {gnss_stamp}   {ndt_stamp}')
                

    def save_data(self):
        with open('pose_comparison_results.txt', 'w', newline='') as file:
            writer = csv.writer(file)
            writer.writerow(['Timestamp_sec', 'Timestamp_nsec', 'X_Diff', 'Y_Diff', 'GNSS_X', 'GNSS_Y', 'NDT_X', 'NDT_Y'])
            for data in self.pose_data:
                writer.writerow(data)


def main(args=None):
    rclpy.init(args=args)
    pose_comparator = PoseComparator()
    pose_comparator.get_logger().info('start compare pose!!!')

    try:
        rclpy.spin(pose_comparator)
    except KeyboardInterrupt:
        pose_comparator.get_logger().info('Shutting down node. Saving data...')
        pose_comparator.save_data()

    pose_comparator.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
